package k8;

import java.text.DecimalFormat;
import java.util.ArrayList;
import java.util.List;

public abstract class SceneNode
{

	// Transformation matrix
	public double[] T;

	// Temporary calculation variables
	private static double f0, f1, uuv, vuv, wuv, xxx, yyy, zzz;

	private static double r00, r01, r02, r10, r11, r12, r20, r21, r22;

	private static double c, oc, s, uu, vv, ww, uv, uw, vw, vvww, uuww, uuvv;

	private List<SceneNode> children;

	/** Creates a new instance of Transformation */
	public SceneNode()
	{
		// Initialise the transformation matrix to an identity
		T = new double[] { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, -1, 0 };

		// Initialise the list of child SceneNodes
		children = new ArrayList<SceneNode>();
	}

	/** Move forward n units. */
	public final void moveForward(double n)
	{
		move(T[2], T[6], T[10], n);
	}

	/** Move right n units. */
	public final void moveRight(double n)
	{
		move(T[0], T[4], T[8], n);
	}

	/** Move up n units. */
	public final void moveUp(double n)
	{
		// move(T[1], T[5], T[9], n);
		move(T[1], T[5], T[9], n);
	}

	/** Move n units along an arbitrary axis <x, y, z>. */
	public final void move(double x, double y, double z, double n)
	{
		T[3] += n * x;
		T[7] += n * y;
		T[11] += n * z;

		transform();

		// Call move(...) on all children
		for (SceneNode child : children)
			child.move(x, y, z, n);
	}

	/** Update world transformation with translation. */
	public final void translate(double x, double y, double z)
	{
		T[3] += x;
		T[7] += y;
		T[11] += z;

		transform();

		// Call translate(...) on all children
		for (SceneNode child : children)
			child.translate(x, y, z);
	}

	/** Roll theta radians. */
	public final void roll(double theta)
	{
		rotate(theta, T[2], T[6], T[10]);
	}

	/** Yaw theta radians. */
	public final void yaw(double theta)
	{
		rotate(theta, T[1], T[5], T[9]);
	}

	/** Pitch theta radians. */
	public final void pitch(double theta)
	{
		rotate(theta, T[0], T[4], T[8]);
	}

	/** Rotate theta radians around an arbitrary axis <u, v, w> */
	public final void rotate(double theta, double u, double v, double w)
	{
		rotate(theta, u, v, w, T[3], T[7], T[11]);
	}

	/**
	 * Rotate theta radians around an arbitrary axis <u, v, w> through the point
	 * <x, y, z>.
	 */
	public final void rotate(double theta, double u, double v, double w, double x,
			double y, double z)
	{
		// It's critical <u, v, w> is a unit vector, otherwise the math goes to
		// shit
		double m = Math.sqrt(u * u + v * v + w * w);
		u /= m;
		v /= m;
		w /= m;

		// Precalculate values common to numerous expressions
		c = Math.cos(theta);
		oc = 1 - c;
		s = Math.sin(theta);
		uu = u * u;
		vv = v * v;
		ww = w * w;

		uv = u * v;
		uw = u * w;
		vw = v * w;

		vvww = vv + ww;
		uuww = uu + ww;
		uuvv = uu + vv;

		uuv = u * (y * v + z * w);
		vuv = v * (x * u + z * w);
		wuv = w * (x * u + y * v);

		r00 = (uu + vvww * c);
		r01 = (uv * oc - w * s);
		r02 = (uw * oc + v * s);
		xxx = (x * vvww - uuv + (uuv - x * vvww) * c + (y * w - z * v) * s);

		r10 = (uv * oc + w * s);
		r11 = (vv + uuww * c);
		r12 = (vw * oc - u * s);
		yyy = (y * uuww - vuv + (vuv - y * uuww) * c + (z * u - x * w) * s);

		r20 = (uw * oc - v * s);
		r21 = (vw * oc + u * s);
		r22 = (ww + uuvv * c);
		zzz = (z * uuvv - wuv + (wuv - z * uuvv) * c + (x * v - y * u) * s);

		rotate();
	}

	/** Auxilary helper method used by the above public rotate method */
	protected final void rotate()
	{
		f0 = T[0];
		f1 = T[4];
		T[0] = r00 * f0 + r01 * f1 + r02 * T[8];
		T[4] = r10 * f0 + r11 * f1 + r12 * T[8];
		T[8] = r20 * f0 + r21 * f1 + r22 * T[8];

		f0 = T[1];
		f1 = T[5];
		T[1] = r00 * f0 + r01 * f1 + r02 * T[9];
		T[5] = r10 * f0 + r11 * f1 + r12 * T[9];
		T[9] = r20 * f0 + r21 * f1 + r22 * T[9];

		f0 = T[2];
		f1 = T[6];
		T[2] = r00 * f0 + r01 * f1 + r02 * T[10];
		T[6] = r10 * f0 + r11 * f1 + r12 * T[10];
		T[10] = r20 * f0 + r21 * f1 + r22 * T[10];

		f0 = T[3];
		f1 = T[7];
		T[3] = r00 * f0 + r01 * f1 + r02 * T[11] + xxx;
		T[7] = r10 * f0 + r11 * f1 + r12 * T[11] + yyy;
		T[11] = r20 * f0 + r21 * f1 + r22 * T[11] + zzz;
		
		transform();

		// Call rotate() on all children
		for (SceneNode child : children)
			child.rotate();
	}

	/** Scale */
	public final void scale(double x, double y, double z)
	{
		T[0] *= x;
		T[1] *= x;
		T[2] *= x;
		T[3] *= x;

		T[4] *= y;
		T[5] *= y;
		T[6] *= y;
		T[7] *= y;

		T[8] *= z;
		T[9] *= z;
		T[10] *= z;
		T[11] *= z;

		transform();

		// Call scale(...) on all children
		for (SceneNode child : children)
			child.scale(x, y, z);
	}

	/** Convenience method to scale equally in all directions */
	public final void scale(double scale)
	{
		scale(scale, scale, scale);
	}

	/**
	 * Adds a SceneNode as a child.
	 * 
	 * @param child
	 *            A SceneNode instance
	 */
	public final void add(SceneNode child)
	{
		children.add(child);
		child.transform(T);
	}

	/**
	 * Removes a SceneNode as a child.
	 * 
	 * @param child
	 *            A SceneNode instance
	 */
	public final void remove(SceneNode child)
	{
		children.remove(child);
	}

	/** Called after the transformation matrix has changed */
	abstract protected void transform();

	/** Apply transformation A to this and children */
	protected final void transform(double[] A)
	{
		f0 = T[0];
		f1 = T[4];
		T[0] = A[0] * f0 + A[1] * f1 + A[2] * T[8];
		T[4] = A[4] * f0 + A[5] * f1 + A[6] * T[8];
		T[8] = A[8] * f0 + A[9] * f1 + A[10] * T[8];

		f0 = T[1];
		f1 = T[5];
		T[1] = A[0] * f0 + A[1] * f1 + A[2] * T[9];
		T[5] = A[4] * f0 + A[5] * f1 + A[6] * T[9];
		T[9] = A[8] * f0 + A[9] * f1 + A[10] * T[9];

		f0 = T[2];
		f1 = T[6];
		T[2] = A[0] * T[2] + A[1] * T[6] + A[2] * T[10];
		T[6] = A[4] * T[2] + A[5] * T[6] + A[6] * T[10];
		T[10] = A[8] * T[2] + A[9] * T[6] + A[10] * T[10];

		f0 = T[3];
		f1 = T[7];
		T[3] = A[0] * f0 + A[1] * f1 + A[2] * T[11] + A[3];
		T[7] = A[4] * f0 + A[5] * f1 + A[6] * T[11] + A[7];
		T[11] = A[8] * f0 + A[9] * f1 + A[10] * T[11] + A[11];

		transform();

		// Call transform(...) on all children
		for (SceneNode child : children)
			child.transform(A);
	}

	/**
	 * Write the the matrix to the logger.
	 */
	public void logMatrix()
	{
		DecimalFormat df = new DecimalFormat("0.000");
		k8.logger.info(df.format(this.T[0]) + " " + df.format(this.T[1]) + " "
				+ df.format(this.T[2]) + " " + df.format(this.T[3]));
		k8.logger.info(df.format(this.T[4]) + " " + df.format(this.T[5]) + " "
				+ df.format(this.T[6]) + " " + df.format(this.T[7]));
		k8.logger.info(df.format(this.T[8]) + " " + df.format(this.T[9]) + " "
				+ df.format(this.T[10]) + " " + df.format(this.T[11]));
		
		k8.logger.info(df.format(r00 * f0 + r01 * f1 + r02 * T[11]) + " " + df.format(xxx));
		k8.logger.info(df.format(r10 * f0 + r11 * f1 + r12 * T[11]) + " " + df.format(yyy));
		k8.logger.info(df.format(r20 * f0 + r21 * f1 + r22 * T[11]) + " " + df.format(zzz));
	}
}
